/*
 * Controller.cpp
 *
 *      Author: Ben, Philippe, Simon en Tom
 */


#include "Robot.h"

int main()
{
   	thread t (server);
   	
	Robot r;
	CanBotLL *c=new CanBotLL();
	//init : zie later
	int arc;
	int dis;
	bool distance_ok = false;
	bool arc_ok=false;
	bool grijpen = true;
	
	vector<Position> v;

	//p = getPacket()
	//r.Robot::UpdateRobot(p);
	//r.Robot::UpdateTarget(p);


	/*TEST CASE*/
	/*Info i;
/*	i.robx = 600;
	i.roby = 200;
	i.robhoek = 40;
	i.doelx = 100;
	i.doely = 400;
	i.garx = 0;
	i.gary = 0;

	Udp_header h;
	Udp_pakket d;
	d.header = h;
	d.groep_a = 0;
	d.info_a = i;
	d.groep_b = 1;
	d.info_b = i;
	Pakket p;
	p.nr = 0;
	p.d = d;

	r.UpdateRobot(p);
	r.UpdateTarget(p);

	arc = r.CalcTurn();
	cout << arc << endl;
	dis = r.CalcDistance();
	cout << dis << endl;*/


	Package a,b,p;

	b=Buffer::get_package();
	a=b;

	for(int i=0;i<2;i++)	//i=0: naar blikje, i=1: naar garage
	{
		if(v.size()==0)	//bij start en als endNode bereikt
		{
			r.updateTarget(b);		
			v.push_back(r.getTarget());		
		}

		r.setTarget(v.front());
		v.erase(v.front());

		while(!distance_ok)
		{
			printf("begin\n");
	
			while(!arc_ok)
			{
				b=Buffer::get_package();
				//Enkel herbereken indien nieuw pakket aankomt
				if(b.nr<=a.nr) {
					continue;
				}
				a=b;
			
				r.UpdateRobot(b);
				arc = r.CalcTurn();
				cout <<"hoek:"<< arc << endl;
				if(arc==0)
				{
					arc_ok = true;
					break;
				}
				r.Turn(arc,c);
			}

			dis = r.CalcDistance();
			cout<<"dist"<<dis<<endl;
			if(dis == 0)
			{
				distance_ok = true;
			
				//if(eindtarget)		
					if(grijpen)
					{
						catchCan();
						grijpen = false;
						r.cargo = true;
					}
					else
					{
						claw(CLAW_OPEN);
						grijpen = true;
						r.cargo = false;
					}
				break;
			}
			r.Drive(dis,c);
			arc_ok = false;
		
		}
	}
	
	//Car in position

	//grijpen
t.join();
}
